
#ifndef RUNE_DETECTOR__PNP_SOLVER_HPP_
#define RUNE_DETECTOR__PNP_SOLVER_HPP_

#include <geometry_msgs/msg/point.hpp>
#include <opencv2/core.hpp>

// STD
#include <array>
#include <vector>


namespace rm_rune_detector
{
    class PnPSolver
    {
    public:
        PnPSolver(
            const std::array<double, 9> &camera_matrix,
            const std::vector<double> &distortion_coefficients);

        // Get 3d position
        // bool solvePnP(const Armor &armor, cv::Mat &rvec, cv::Mat &tvec);

        // Calculate the distance between armor center and image center
        float calculateDistanceToCenter(const cv::Point2f &image_point);

    private:
        cv::Mat camera_matrix_;
        cv::Mat dist_coeffs_;

        // Unit: mm
        static constexpr float SMALL_ARMOR_WIDTH = 135;
        static constexpr float SMALL_ARMOR_HEIGHT = 55;
        static constexpr float LARGE_ARMOR_WIDTH = 225;
        static constexpr float LARGE_ARMOR_HEIGHT = 55;

        // Four vertices of armor in 3d
        std::vector<cv::Point3f> small_armor_points_;
        std::vector<cv::Point3f> large_armor_points_;
    };

} // namespace rm_auto_aim

#endif // RUNE_DETECTOR__PNP_SOLVER_HPP_
